#include "IMU_Task.h"
#include "kalman_filter.h"
#include "BMI088driver.h"
#include "pid.h"
#include "bsp_imu_pwm.h"
#include "cmsis_os.h"

#define correct_Time_define 1000    //上电去0飘 1000次取平均
#define temp_times 300       //探测温度阈值

#define TEMPERATURE_PID_KP 1600.0f //kp of temperature control PID 
#define TEMPERATURE_PID_KI 0.2f    //ki of temperature control PID 
#define TEMPERATURE_PID_KD 5.0f    //kd of temperature control PID 

#define TEMPERATURE_PID_MAX_OUT 4500.0f  //max out of temperature control PID 
#define TEMPERATURE_PID_MAX_IOUT 600.0f //max iout of temperature control PID 
#define Destination_TEMPERATURE 40.f

extern SPI_HandleTypeDef hspi1;

uint8_t attitude_flag=0;
uint32_t correct_times=0;
//static float ddt = 0, dt = 0, t = 0;

const float imu_temp_PID[3] = {TEMPERATURE_PID_KP, TEMPERATURE_PID_KI, TEMPERATURE_PID_KD};
pid_type_def imu_temp_pid;

IMU_Data_t IMU_Data1;

void Imu_Init(void)
{
	IMU_QuaternionEKF_Init(10, 0.001f,
                           10000000,1, 0.001f,0); //ekf初始化
	PID_init(&imu_temp_pid, PID_POSITION, imu_temp_PID,
             TEMPERATURE_PID_MAX_OUT, TEMPERATURE_PID_MAX_IOUT);
	HAL_TIM_PWM_Start(&htim10, TIM_CHANNEL_1);
	while(BMI088_init()){}
}

/**
  * @brief          bmi088温度控制
  * @param[in]      argument: NULL
  * @retval         none
  */
void IMU_Temperature_Ctrl(IMU_Data_t *IMU1)
{
	uint16_t tempPWM;
	//pid calculate. PID计算
	PID_calc(&imu_temp_pid, IMU1->temp, Destination_TEMPERATURE);
	if (imu_temp_pid.out < 0.0f)
	{
		imu_temp_pid.out = 0.0f;
	}
	tempPWM = (uint16_t)imu_temp_pid.out;
    imu_pwm_set(tempPWM);
}

void INS_Task(IMU_Data_t *IMU1)
{
    static uint32_t count = 0;

    // ins update
    if ((count % 1) == 0)
    {
        BMI088_read(IMU1->gyro, IMU1->accel, &IMU1->temp);
        
        if(attitude_flag==2)  //ekf的姿态解算
        {
			IMU1->gyro[0]-=IMU1->gyro_correct[0];   //减去陀螺仪0飘
			IMU1->gyro[1]-=IMU1->gyro_correct[1];
			IMU1->gyro[2]-=IMU1->gyro_correct[2];

			//===========================================================================
			//ekf姿态解算部分
			//HAL_GPIO_WritePin(GPIOC,GPIO_PIN_9,GPIO_PIN_SET);
			IMU_QuaternionEKF_Update(
									-IMU1->gyro[0],-IMU1->gyro[1],IMU1->gyro[2],
									-IMU1->accel[0],-IMU1->accel[1],IMU1->accel[2]);
			//HAL_GPIO_WritePin(GPIOC,GPIO_PIN_9,GPIO_PIN_RESET);
			//=============================================================================== 
			//ekf获取姿态角度函数
			IMU1->pitch=Get_Pitch(); //获得pitch
			IMU1->roll=Get_Roll();//获得roll
			IMU1->yaw=Get_Yaw();//获得yaw
			memcpy(IMU1->q, QEKF_INS.q, 16);
			//==============================================================================
        }
        else if(attitude_flag==1)   //状态1 开始1000次的陀螺仪0飘初始化
        {
            //gyro correct
            IMU1->gyro_correct[0]+= IMU1->gyro[0];
            IMU1->gyro_correct[1]+= IMU1->gyro[1];
            IMU1->gyro_correct[2]+= IMU1->gyro[2];

            correct_times++;
            if(correct_times>=correct_Time_define)
            {
				IMU1->gyro_correct[0]/=correct_Time_define;
				IMU1->gyro_correct[1]/=correct_Time_define;
				IMU1->gyro_correct[2]/=correct_Time_define;

				attitude_flag=2; //go to 2 state
            }

            IMU1->gyro_correct[0]=0.00364531903f;
			IMU1->gyro_correct[1]=0.000194943437f;
			IMU1->gyro_correct[2]=0.002902837f;

            attitude_flag=2; //go to 2 state
        }
    }
// temperature control
    if ((count % 10) == 0)
    {
        // 100hz 的温度控制pid
        IMU_Temperature_Ctrl(IMU1);
//        attitude_flag=1;  //go to correct state
        static uint32_t temp_Ticks=0;
        if((fabsf(IMU1->temp-Destination_TEMPERATURE)<0.5f)&&attitude_flag==0) //接近额定温度之差小于0.5° 开始计数
        {
            temp_Ticks++;
            if(temp_Ticks>temp_times)   //计数达到一定次数后 才进入0飘初始化 说明温度已经达到目标
            {
                attitude_flag=1;  //go to correct state
            }
        }
    }
    count++;
}


